﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Graphics;
using ConfigReader;

namespace XNAViewerSample.Physics
{
    public class RotationalSpring
    {
        #region Fields
        protected Vector3 trackRotation;
        protected Vector3 objectRotation;
        protected String boneId;
        protected float mass;
        protected float stiffness;
        protected float damping;
        protected Vector3 clamp;
        protected Vector3 velocity;
        protected List<SoftBody.FType> forces;

        public Vector3 TrackRotation
        {
            get { return trackRotation; }
            set { trackRotation = value; }
        }
        public Vector3 ObjectRotation
        {
            get { return objectRotation; }
            set { objectRotation = value; }
        }
        public String BoneID
        {
            get { return boneId; }
            set { boneId = value; }
        }
        public float Mass
        {
            get { return mass; }
            set { mass = value; }
        }
        public float Stiffness
        {
            get { return stiffness; }
            set { stiffness = value; }
        }
        public float Damping
        {
            get { return damping; }
            set { damping = value; }
        }
        public Vector3 Clamp
        {
            get { return clamp; }
            set { clamp = value; }
        }
        public List<SoftBody.FType> Forces
        {
            get { return forces; }
            set { forces = value; }
        }

        #endregion

        public RotationalSpring()
        {
            trackRotation = Vector3.Zero;
            objectRotation = Vector3.Zero;
            forces = new List<SoftBody.FType>();
        }

        public Matrix ConvertToLocal(Matrix boneTransform)
        {
            Matrix m = Matrix.Identity;

            // Decompose local bone transform into its components
            Vector3 scale, translation;
            Quaternion rotation;
            boneTransform.Decompose(out scale, out rotation, out translation);

            // create a new vector transformed accdg to bone orientation
            Vector3 o = Vector3.Transform(objectRotation, Matrix.CreateFromQuaternion(rotation));
            
            // create a local space rotation            
            m = Matrix.CreateFromYawPitchRoll(MathHelper.ToRadians(o.Y),
                                              MathHelper.ToRadians(o.X),
                                              MathHelper.ToRadians(o.Z));

            return m;
        }

        public bool Update(float elapsedTime, Vector3 rotation, Vector3 translation)
        {
            Vector3 force = Vector3.Zero;
            Vector3 direction = Vector3.Zero;
            bool applyForce = false;
           
            // sum all force generators
            foreach (SoftBody.FType fg in forces)
            {
                switch (fg)
                {
                    case SoftBody.FType.Inertia:
                        // Inertial force is simulated by creating a counter
                        // acting force causing an opposite displacement from
                        // model movement. This displacement is passed to the
                        // spring force calculation

                        // Handle model rotation first
                        direction = trackRotation - rotation;
                        if (direction != Vector3.Zero)
                        {
                            trackRotation = rotation;
                            Vector3 v = objectRotation;
                            v.X = MathHelper.Clamp(direction.X + v.X, -clamp.X, clamp.X);
                            v.Y = MathHelper.Clamp(direction.Y + v.Y, -clamp.Y, clamp.Y);
                            v.Z = MathHelper.Clamp(direction.Z + v.Z, -clamp.Z, clamp.Z);

                            objectRotation = v;
                            applyForce = true;
                        }
                        break;
                    case SoftBody.FType.Gravity:
                        // Gravity is simulated as a force in the negative Y
                        // direction, depending on mass of object
                        break;
                }
            }
            
            if (!applyForce)
            {
                // Update only for discernible movements
                if ((Math.Abs(objectRotation.X) < 0.001f) &&
                    (Math.Abs(objectRotation.Y) < 0.001f) &&
                    (Math.Abs(objectRotation.Z) < 0.001f)) 
                    return false;

                // add the spring force due to displacement
                Vector3 springForce = (-1.0f * stiffness * objectRotation) - (damping * velocity);
                force += springForce;

                // Integrate to get final velocity and displacement
                Vector3 acceleration = force / mass;
                velocity += acceleration * elapsedTime;
                objectRotation += velocity * elapsedTime;

                //return ((Math.Abs(objectRotation.X) > 0.001f) || 
                //        (Math.Abs(objectRotation.Y) > 0.001f) || 
                //        (Math.Abs(objectRotation.Z) > 0.001f)) ? true : false;
            }
            return true;
        }
    }
}
